import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
import sys


class ImuReader(Node):

    def __init__(self):
        super().__init__('imu_reader')
        self.publisher_ = self.create_publisher(Imu, '/cranebot/imu', 10)
        self.subscription = self.create_subscription(
            Quaternion,
            '/cranebot/move',
            self.listener_callback,
            10
        )
        self.subscription
        self.operation_tag = "pause"
        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.rotate_move = 0
        self.raise_move = 0
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.handle_cranebot_pose)
        self.operation_start_time_tag = self.get_clock().now().nanoseconds
        self.operation_relay_time_tag = self.get_clock().now().nanoseconds

    def listener_callback(self, msg):
        if ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 0.0)):
            self.operation_tag = "pause"
            #print("PAUSE")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 1.0) & (msg.z == 0.0)):      
            self.operation_tag = "rotate_left"
            #print("extend_forward")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == -1.0) & (msg.z == 0.0)):      
            self.operation_tag = "rotate_right"
            #print("extend_back")
        elif ((msg.w == 1.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 0.0)):    
            self.operation_tag = "raise_up"
            #print("lift_up")
        elif ((msg.w == -1.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 0.0)):     
            self.operation_tag = "raise_down"
            #print("lift_down")
        self.operation_start_time_tag = self.get_clock().now().nanoseconds
        

    def handle_cranebot_pose(self):
        self.operation_time_tag = self.get_clock().now().nanoseconds
        operation_period = (self.operation_time_tag-self.operation_relay_time_tag)/1000000000
        if self.operation_tag == "pause":
            pass
        elif self.operation_tag == "rotate_left":
            self.rotate_move += operation_period * 5
        elif self.operation_tag == "rotate_right":
            self.rotate_move -= operation_period * 5
        elif self.operation_tag == "raise_up":
            self.raise_move += operation_period * 5
        elif self.operation_tag == "raise_down":
            self.raise_move -= operation_period * 5
        self.operation_relay_time_tag = self.operation_time_tag
        print(self.rotate_move, self.raise_move)
        msg = Imu()
        msg.angular_velocity.x = 0.0
        msg.angular_velocity.y = float(self.raise_move)
        msg.angular_velocity.z = float(self.rotate_move)
        msg.linear_acceleration.x = 0.0
        msg.linear_acceleration.y = 0.0
        msg.linear_acceleration.z = 0.0
        msg.orientation.x = 0.0
        msg.orientation.y = 0.0
        msg.orientation.z = 0.0
        msg.orientation.w = 0.0
        self.publisher_.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    imu_reader = ImuReader()
    rclpy.spin(imu_reader)
    imu_reader.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

